#include <iostream>
using namespace std;
#include <stdio.h>
#include <cmath>

#include <mex.h>
#include "mexFileIO.h"

#include <vector>
#include <algorithm>
#include <queue>
#include <limits>
#include <map>
using namespace std;
#include <mexFileIO.h>
#include <szMexUtility.h>
#include <szMexUtilityTemplate.h>
#include <szMyNeighborOp.h>
#include <szMiscOperations.h>
#include <szConvexHull2D.h>
#include <ContourEQW.h>
#include <szParticleF.h>
#include <Triangulation.h>
#include <FragmentInfo.h>
#include <TriangulationHelper.h>
#include <ConvexityMeasure.h>

vector<int>
separateTree(vector<CParticleF>& points,
			 vector<int>& tree,
			 int removeId,
			 const int* dims)
{
	vector<Node<int>*> nodes;
	for(int i=0; i<points.size(); ++i)
	{
		nodes.push_back(makeset(i));
	}
	for(int i=0; i<dims[0]; ++i)
	{
		if(i != removeId)
		{
			int j1 = GetData2(tree, i, 0, dims[0], dims[1], -1);
			int j2 = GetData2(tree, i, 1, dims[0], dims[1], -1);
			merge(nodes[j1], nodes[j2]);
		}
	}
	vector<Node<int>*> cl = clusters(nodes);
	vector<int> labels(points.size(), 0);
	for(int i=0; i<nodes.size(); ++i)
	{
		int k = distance(cl.begin(), find(cl.begin(), cl.end(), findset(nodes[i])));
		labels[i] = k + 1;
	}
	for(int i=0; i<nodes.size(); ++i)
	{
		delete nodes[i];
	}
	return labels;
}

float
intraDistance(vector<CParticleF>& points)
{
	if(points.size()==0) return 0.0f;

	CParticleF mean(0.0f, 0.0f);
	for(int i=0; i<points.size(); ++i)
	{
		mean.m_X += points[i].m_X;
		mean.m_Y += points[i].m_Y;
	}
	mean.m_X = mean.m_X / points.size();
	float dsum = 0;
	for(int i=0; i<points.size(); ++i)
	{
		dsum += Distance(mean, points[i]);
	}
	return dsum / points.size();
}

float
interDistance(vector<CParticleF>& left, vector<CParticleF>& right)
{
	/*if(left.size()==0 || right.size()==0) return std::numeric_limits<float>::quiet_NaN();

	CParticleF lmean(0.0f, 0.0f);
	for(int i=0; i<left.size(); ++i)
	{
		lmean.m_X += left[i].m_X;
		lmean.m_Y += left[i].m_Y;
	}
	lmean.m_X = lmean.m_X / left.size();
	CParticleF rmean(0.0f, 0.0f);
	for(int i=0; i<right.size(); ++i)
	{
		rmean.m_X += right[i].m_X;
		rmean.m_Y += right[i].m_Y;
	}
	rmean.m_X = lmean.m_X / right.size();
	return Distance(lmean, rmean) * (2.5f* (sqrt((float)left.size() * right.size()) / (left.size() + right.size())-0.1f));*/
	vector<CParticleF> points = left;
	points.insert(points.end(), right.begin(), right.end());
	return intraDistance(points);
}

float
separationMeasure(vector<CParticleF>& left, vector<CParticleF>& right)
{
	return (intraDistance(left) + intraDistance(right)) / interDistance(left, right);
}

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{   
	if (nrhs < 3 || nlhs < 0)
	{
		mexErrMsgTxt("Usage: TestBed(P, tree, id)");
		return;
	}
	//Points
	vector<CParticleF> points;
	{
		vector<float> P0;
		mxClassID classIdP;
		int ndimP;
		const int* dimsP;
		LoadData(P0, prhs[0], classIdP, ndimP, &dimsP);
		points = vector2particle(P0, dimsP);
	}
	//edge info (indices to the points)
	vector<int> tree;
	const int* dimsT;
	{
		mxClassID classIdT;
		int ndimT;
		LoadData(tree, prhs[1], classIdT, ndimT, &dimsT);
		for(int i=0; i<tree.size(); ++i) tree[i]--; //one index to zero index
	}

	int removeEdge = -1.0;	
	if(nrhs>=2) 
	{
		mxClassID classMode;
		ReadScalar(removeEdge,prhs[2],classMode);
		removeEdge-=1; //one index to zero index
	} 

	vector<int> label = separateTree(points, tree, removeEdge, dimsT);
	vector<CParticleF> left;
	vector<CParticleF> right;
	for(int i=0; i<points.size(); ++i)
	{
		if(label[i]==1) left.push_back(points[i]);
		else if(label[i]==2) right.push_back(points[i]);
	}

	float separation = separationMeasure(left, right);
	printf("%d: Separation (%d, %d) = %f\n", removeEdge+1, left.size(), right.size(), separation);

	mexUnlock();
}

